﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Coding4Fun.Kinect.Wpf;
using System.Windows;
using System.Windows.Input;
using System.Windows.Media.Imaging;
using System.Windows.Media;

namespace PRScreenWithKinect
{
    class Kinect
    {
        public Kinect(int width, int height)//, MainWindow main)
        {

            this.width = width;
            this.height = height;
            //this.mainPage = main;
            initialize();
        }

        const float MaxDepthDistance = 4095; // max value returned
        const float MinDepthDistance = 850; // min value returned
        const float MaxDepthDistanceOffset = MaxDepthDistance - MinDepthDistance;

        //MainWindow mainPage;
        private int width, height;
        Gesture gesture;
        KinectSensor sensor;

        private Skeleton[] allSkeletons;

        Joint lHand, rHand, spine, rShdler, lShdler, cShdler, lWrist, rHip;
        //byte[] depthFrame32 = new byte[320 * 240 * 4];
        const int RED_IDX = 2;
        const int GREEN_IDX = 1;
        const int BLUE_IDX = 0;

        private void initialize()
        {
            if (KinectSensor.KinectSensors.Count > 0) //check whether one or more kinect are connected
            {
                sensor = KinectSensor.KinectSensors[0];
                if (sensor.Status == KinectStatus.Connected)
                {
                    sensor.ColorStream.Enable(ColorImageFormat.RgbResolution640x480Fps30);
                    sensor.DepthStream.Enable(DepthImageFormat.Resolution320x240Fps30);

                    var parameters = new TransformSmoothParameters
                    {
                        Smoothing = 0.7f,
                        Correction = 0.7f,
                        Prediction = 0.7f,
                        JitterRadius = 0.03f,
                        MaxDeviationRadius = 0.03f
                    };
                    sensor.SkeletonStream.Enable(parameters);
                    gesture = new Gesture(width, height, this);

                    sensor.AllFramesReady += new EventHandler<AllFramesReadyEventArgs>(sensor_AllFramesReady);
                    sensor.DepthFrameReady += new EventHandler<DepthImageFrameReadyEventArgs>(sensor_DepthFrameReady);
                    //DepthFrameReady is for checking Human in/out
                    sensor.Start();
                }
            }
        }

        private int chkHuman = 0;
        private Boolean human = false;
        void sensor_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        {
            if (human == true)//human out is mean that skeletonFrame is stop activate event but
                              //depthFrame is still.
            {
                chkHuman += 1;
                if (chkHuman > 30)//if skeletonFrame is still running this chkHuman will never reach 30
                                  //because every skeletonFrameReady, chkHuman will be set to 0.
                {
                    Handler.DoHumanOut(new KinectEventArgs()); 
                    chkHuman = 0;
                    human = false;
                }
            }
        }

        //int pushState = 0;
        void sensor_AllFramesReady(object sender, AllFramesReadyEventArgs e)
        {


            using (ColorImageFrame colorFrame = e.OpenColorImageFrame())
            {
                if (colorFrame != null)
                {
                    byte[] pixels = new byte[colorFrame.PixelDataLength];
                    colorFrame.CopyPixelDataTo(pixels);

                    int stride = colorFrame.Width * 4;
                    BitmapSource source =
                        BitmapSource.Create(colorFrame.Width, colorFrame.Height,
                        96, 96, PixelFormats.Bgr32, null, pixels, stride);

                    KinectEventArgs eColorFrame = new KinectEventArgs();
                    eColorFrame.imageSrc = source;

                    Handler.DoColorFrame(eColorFrame);
                }
            }

            KinectEventArgs eKinect = new KinectEventArgs();

            Skeleton skelData = GetFirstSkeleton(e);
            if (skelData == null)
            {
                return;
            }

            if (!human)
            {
                Handler.DoHumanIn(eKinect);
                human = true;
            }
            chkHuman = 0; //set to 0 so it will not reach 30 and HumanOut Event will be actived


            lHand = skelData.Joints[JointType.HandLeft];
            rHand = skelData.Joints[JointType.HandRight];
            spine = skelData.Joints[JointType.Spine];
            rShdler = skelData.Joints[JointType.ShoulderRight];
            lShdler = skelData.Joints[JointType.ShoulderLeft];
            cShdler = skelData.Joints[JointType.ShoulderCenter];
            lWrist = skelData.Joints[JointType.WristLeft];
            rHip = skelData.Joints[JointType.HipRight];

            eKinect.LHandPosition = getLHandPosition(width, height);
            eKinect.RHandPosition = getRHandPosition(width, height);
            eKinect.position = rHand.Position.Y;
            Handler.DoRHandMove(eKinect);
            Handler.DoLHandMove(eKinect);


            DepthImageFrame depthFrame = e.OpenDepthImageFrame();
            if (depthFrame == null) return;

            gesture.updateMovement(lHand, rHand, spine, rHip, depthFrame); //*** update data in Gesture for calculate and detect user gesture

            int rHandVSwipe = gesture.rHandVSwipe();
            int lHandVSwipe = gesture.lHandVSwipe();

            if (gesture.rHandHSwipe() == -1) Handler.DoSwipeLeft(eKinect);

            if (rHandVSwipe == 1 || lHandVSwipe == 1) { Handler.DoSwipeUp(eKinect);}
            if (rHandVSwipe == -1 || lHandVSwipe == -1) { Handler.DoSwipeDown(eKinect);}

            //else if (handGes.rHandSlide() == -1) Handler.DoSwipeLeft(eKinect);

            if (gesture.lHandHSwipe() == 1) Handler.DoSwipeRight(eKinect);

            //if (handGes.HandSlide() == 1) ;

            //else if (handGes.rHandSlide() == -1) ;

            if (this.rHandActive == true && rHand.Position.Y < rHip.Position.Y-0.1f)
            {
                Handler.DoRHandIdle(eKinect);
                this.rHandActive = false;
            }
            else if (this.rHandActive == false && rHand.Position.Y > rHip.Position.Y-0.1f)
            {
                Handler.DoRHandActive(eKinect);
                this.rHandActive = true;
            }

            if (this.lHandActive == true && lHand.Position.Y < rHip.Position.Y - 0.1f)
            {
                Handler.DoLHandIdle(eKinect);
                this.lHandActive = false;
            }
            else if (this.lHandActive == false && lHand.Position.Y > rHip.Position.Y - 0.1f)
            {
                Handler.DoLHandActive(eKinect);
                this.lHandActive = true;
            }





        }
        Boolean rHandActive = true;
        Boolean lHandActive = true;
        

        private Skeleton GetFirstSkeleton(AllFramesReadyEventArgs e) //get the first player
        {
            using (SkeletonFrame skeletonFrame = e.OpenSkeletonFrame())
            {
                if (skeletonFrame != null)
                {
                    if ((this.allSkeletons == null) || (this.allSkeletons.Length != skeletonFrame.SkeletonArrayLength))
                    {
                        this.allSkeletons = new Skeleton[skeletonFrame.SkeletonArrayLength];
                    }
                }
                else { return null; }

                skeletonFrame.CopySkeletonDataTo(this.allSkeletons);


                Skeleton skel = (from s in allSkeletons
                            where s.TrackingState == SkeletonTrackingState.Tracked
                            orderby s.Position.Z ascending 
                            select s).FirstOrDefault();
                return skel;

            }
        }

        private int x, y, index, testDepth;
        //private Boolean onClicked = false, onPosition = false;


        //void nui_DepthFrameReady(object sender, DepthImageFrameReadyEventArgs e)
        //{
        //    //OLD//PlanarImage Image = e.ImageFrame.Image;
        //    DepthImageFrame depthFrame = e.OpenDepthImageFrame();
        //    DepthImagePoint rHandDepthPoint = depthFrame.MapFromSkeletonPoint(rHand.Position);
        //    DepthImagePoint spineDepthPoint = depthFrame.MapFromSkeletonPoint(spine.Position);
        //                ////index คือ ตำแหน่งของ Array ที่เก็บข้อมูลของ pixel นั้นๆ
        //    ////rHandIndex ก็คือ ตำแหน่งของ Array ที่เก็บข้อมูล pixel ที่เป็นจุดมือขวา ตามตำแหน่ง RightHand ของ Skeleton
        //    ////ซึ่งสำหรับข้อมูล 16 bit/pixel นี้ Array ที่เก็บข้อมูล pixel นี้คือ Image.bits[rHandIndex] และ Imagel.bits[rHandIndex+1] 
        //    //int rHandIndex = getDepthIndexFromDepth(rHand);
        //    //int spineIndex = getDepthIndexFromDepth(spine);

        //    //int spineDepth = getRealDepth(spineIndex, Image.Bits);
        //    //int rHandDepth = getRealDepth(rHandIndex, Image.Bits);
        //    handGes.updateMovement(lHand, rHand, spine, depthFrame);
        //    ////mainPage.setOKLabel(getDisplayPosition(rHand, width, height)+"");


        //    //mainPage.setOKLabel("TRUE") ;
        //    if (handGes.rHandPushed(depthFrame) == 1) ;
        //    //mainPage.setLabel("Push");
        //    else if (handGes.rHandPushed(depthFrame) == -1) ;
        //    //mainPage.setLabel("Pull");
        //    if (handGes.rHandSlide() == 1) ;
        //    //mainPage.setLabel("Slide Right");
        //    else if (handGes.rHandSlide() == -1) ;
        //    //mainPage.setLabel("Slide Left");

        //    //else mainPage.setOKLabel("FALSE");  

        //    //byte[] convertedDepthFrame = convertDepthFrame(Image.Bits);
        //    //mainPage.depthDraw(convertedDepthFrame, Image);
        //}


        public int getDepthIndexOfJoint(Joint joint, DepthImageFrame depth) //return index of joint's position which store in depthframe array <array of byte> 
        {
            DepthImagePoint depthPoint = depth.MapFromSkeletonPoint(joint.Position);
            int index = depthPoint.Y * 320 + depthPoint.X;
            return index;
        }



        Joint scaledJoint;


        ////แบบ Parallel
        private Point getDisplayPosition(Joint joint)
        {
            scaledJoint = joint.ScaleTo(width, height, 0.45f, 0.3f);
            float depthX, depthY;
            depthX = scaledJoint.Position.X;
            depthY = scaledJoint.Position.Y;
            ////Beta Code/////////
            //SkeletonPoint skelPoint = new SkeletonPoint();
            //skelPoint.X = 0;
            //Joint j = new Joint();
            //j.Position = skelPoint;
            return new Point(Math.Floor(depthX), Math.Floor(depthY));
        }

        public Point getLHandPosition(int width, int height)
        {
            /////////////////////ทำให้หัวไหล่ขวา เป็นตำแหน่งกลางจอเสมอ////////////////////////////////////
            SkeletonPoint skelPoint = new SkeletonPoint(); //assign new skelPoint to create new position of rHand
            skelPoint = lHand.Position;                    //cuz rHand.Position.X can't be directly modified .
            skelPoint.X = lHand.Position.X - cShdler.Position.X+0.05f;
            skelPoint.Y = lHand.Position.Y - lShdler.Position.Y+0.1f;

            Joint newLHand = new Joint();
            newLHand.Position = skelPoint;

            return getDisplayPosition(newLHand);
        }

        public Point getLHandPosition(int width, int height, Joint lHand )
        {
            /////////////////////ทำให้หัวไหล่ขวา เป็นตำแหน่งกลางจอเสมอ////////////////////////////////////
            SkeletonPoint skelPoint = new SkeletonPoint(); //assign new skelPoint to create new position of rHand
            skelPoint = lHand.Position;                    //cuz rHand.Position.X can't be directly modified .
            skelPoint.X = lHand.Position.X - cShdler.Position.X + 0.05f;
            skelPoint.Y = lHand.Position.Y - lShdler.Position.Y + 0.1f;

            Joint newLHand = new Joint();
            newLHand.Position = skelPoint;

            return getDisplayPosition(newLHand);
        }

        public Point getRHandPosition(int width, int height)
        {
            /////////////////////ทำให้หัวไหล่ขวา เป็นตำแหน่งกลางจอเสมอ////////////////////////////////////
            SkeletonPoint skelPoint = new SkeletonPoint(); //assign new skelPoint to create new position of rHand
            skelPoint = rHand.Position;                    //cuz rHand.Position.X can't be directly modified .
            skelPoint.X = rHand.Position.X - cShdler.Position.X+0.05f;
            skelPoint.Y = rHand.Position.Y - rShdler.Position.Y+0.1f;

            Joint newRHand = new Joint();
            newRHand.Position = skelPoint;

            return getDisplayPosition(newRHand);

            /////////// Kinect SDK Beta //////////////
            ///////////////////////ทำให้หัวไหล่ขวา เป็นตำแหน่งกลางจอเสมอ////////////////////////////////////
            //Microsoft.Research.Kinect.Nui.Vector v = new Microsoft.Research.Kinect.Nui.Vector();
            //v = rHand.Position;
            //v.X = rHand.Position.X - rShdler.Position.X;

            //Joint newRHand = new Joint();
            //newRHand.Position = v;
            ///// ใช้วิธี สร้าง Joint ตัวใหม่ ที่ลอกแบบมาจาก rHand แต่ตำแหน่งแกน X จะอ้างอิงจาก Joint ของหัวไหล่ขวา ///
            ///////////////////////////////////////////////////////////////////////////////////////

            //return getDisplayPosition(newRHand, width, height);
        }

        public Point getRHandPosition(int width, int height, Joint rHand)
        {
            /////////////////////ทำให้หัวไหล่ขวา เป็นตำแหน่งกลางจอเสมอ////////////////////////////////////
            SkeletonPoint skelPoint = new SkeletonPoint(); //assign new skelPoint to create new position of rHand
            skelPoint = rHand.Position;                    //cuz rHand.Position.X can't be directly modified .
            skelPoint.X = rHand.Position.X - cShdler.Position.X + 0.05f;
            skelPoint.Y = rHand.Position.Y - rShdler.Position.Y + 0.1f;

            Joint newRHand = new Joint();
            newRHand.Position = skelPoint;

            return getDisplayPosition(newRHand);

        }

        public void uninitialize()
        {
            sensor.Stop();
        }

        public int getRealDepth(int index, short[] rawDepthData)    //no need to use this method in normal case. 
        {                                                           //Kinect Lib already have function (DepthImagePoint).Depth
            if (index < 0) return 0;
            else
                return rawDepthData[index] >> DepthImageFrame.PlayerIndexBitmaskWidth;
        }

        public void doMenuSelected(bool flag)
        {
            if (flag == true)
            {
                gesture.verticalStat = "down";
            }
            else
            {
                gesture.verticalStat = "up";
            }
            
        }

    }
}